/*utf-8*/
//Please open this file with UTF-8 coding to decode this file correctlly.

//C plus plus standard headfile
#include <thread>
#include <vector>
#include <iostream>
#include <fstream>
#include <string>
#include <chrono>

//C standard headfile for c plus plus
#include <cstddef>
#include <cstdio>
#include <cstdlib>

#include "mpu-9250.h"

using namespace std;

int main(int argc, char *argv[])
{
    hg::imu::mpu_9250 mpu9250;

    if(argc<2){
        std::cerr<<"Usage "<< argv[0] <<" /dev/i2c-1 0x68 0x0c 2 500\n";
        std::cerr<<"Usage "<< argv[0] <<" /dev/i2c-2 2> /data/local/hg/mpu9250_offsets.ini"<<std::endl;
        return 1;
    }

    if(argc>=2) mpu9250.i2c_bus = argv[1];
    if(argc>=3)
        mpu9250.i2c_mpu_9250_address = std::strtol(argv[2],nullptr,0);
    if(argc>=4)
        mpu9250.i2c_ak8963_address   = std::strtol(argv[3],nullptr,0);
    if(argc>=5)
        mpu9250.accel_fs             = std::strtol(argv[4],nullptr,0);
    if(argc>=6)
        mpu9250.gyro_fs              = std::strtol(argv[5],nullptr,0);

    if( !mpu9250.open() ){
        return 1;
    }

    const int count=500;
    double angular_sums[3]={0};
    double angular_offsets[3];
    int mag_count=5;
    bool has_magnetic = false;

    std::cout<<"time passed,";
    std::cout<<"Acceleration data[x],Acceleration data[y],Acceleration data[z],";
    std::cout<<"Angular velocity data[x],Angular velocity data[y],Angular velocity data[z],";
    std::cout<<"Magnetic field data[x],Magnetic field data[y],Magnetic field data[z],";
    std::cout<<std::endl;

    auto start = std::chrono::high_resolution_clock::now();
 
	
//	mpu9250.read_acceleration();
//	for(int i=0;i<3;++i){
//		std::cout<<mpu9250.acceler_values[i]<<",";
//	}


	while(true){
        /*if(mag_count>=20){
            mpu9250.read_acceleration();
            mpu9250.read_angular_velocity();
            has_magnetic = mpu9250.read_magnetic_field();

            if(has_magnetic){
                mag_count=0;
            }
        }else{
            mpu9250.read_acceleration();
            mpu9250.read_angular_velocity();
            has_magnetic = false;
            mag_count+=1;
        }*/

		mpu9250.read_acceleration();
        mpu9250.read_angular_velocity();
		
        auto end = std::chrono::high_resolution_clock::now();
        std::chrono::duration<double> diff = end - start;

        //[1]
        std::cout<< diff.count() <<",";

        //[234]
        for(int i=0;i<3;++i){
            std::cout<<mpu9250.acceler_values[i]<<",";
        }

        //[567]
        for(int i=0;i<3;++i){
            std::cout<<mpu9250.angular_values[i]<<",";
        }

        //[8910]
        for(int i=0;i<3;++i){
            std::cout<<mpu9250.magnet_values[i]<<",";
        }

        std::cout<<std::endl;
        //std::this_thread::sleep_for(std::chrono::milliseconds(100));// Hz
    }

    std::cout<<"Angular velocity offsets data:\n";

    for(int i=0;i<3;++i){
        angular_offsets[i] = angular_sums[i]/count;
        std::cout<<"XYZ"[i]<<":"<<angular_offsets[i]<<"\t";
    }

    std::cout<<std::endl;

    const char *out =
            "[angular_offsets]\n"
            "x=%lf\n"
            "y=%lf\n"
            "z=%lf\n";
    fprintf( stderr, out, angular_offsets[0],angular_offsets[1],angular_offsets[2] );

    return 0;
}


